/*
 * takeSnapshot.cpp
 *
 *  Created on: Nov 26, 2012
 *      Author: Matthijs
 */
#include <camera/camera.h>


/**
 *	Take a single high-res snapshot and parse it for tag
 *	identification.
 *
 *	The request takes two parameters: left and right. These determine
 *	which camera should take the snapshot, with both also being possible.
 *
 *	After taking the snapshot, the image is processed, the square-detector
 *	will attempt to find red squares, and the tag-detector will try to identify
 *	the tags and subsequently send a tag message to the SLAM node.
 *
 *	Since all this takes a second or two, it's a really, really good idea to keep
 *	the robot standing still untill the service returns.
 */
bool Camera::takeSnapshot (sensor::Snapshot::Request   &req,
		 	 	 	 	   sensor::Snapshot::Response  &res)
{
	cv::Mat image;
	if (req.left)
	{
		captureLeft   = new cv::VideoCapture( 0 );
		captureLeft  -> set(CV_CAP_PROP_FRAME_WIDTH, 	800);
		captureLeft  -> set(CV_CAP_PROP_FRAME_HEIGHT, 	600);
		(*captureLeft) >> image;
		(*captureLeft) >> image;
		findSquares(image, 1.52, true);	// Image, angle, identify the tag.
//		imwrite("snapshots/left.jpg", image);
		captureLeft -> release();
	}
	if (req.right)
	{
		captureRight  = new cv::VideoCapture( 1 );
		captureRight -> set(CV_CAP_PROP_FRAME_WIDTH,   	800);
		captureRight -> set(CV_CAP_PROP_FRAME_HEIGHT, 	600);
		(*captureRight) >> image;
		(*captureRight) >> image;
		findSquares(image, -1.52, true);
//		imwrite("snapshots/right.jpg", image);
		captureRight -> release();
	}
	ROS_INFO("Took a snapshot");
	return true;
}
